import json
from loguru import logger
import rclpy
from rclpy.node import Node
from std_msgs.msg import String, Char

# local
from Charger_C3_protocol import *

# Sim_Cmd_Control = False
Sim_Cmd_Control = True


class Charger_Ros(Node):
    def __init__(self, ChargerServer):
        super().__init__("charger_server")
        self.ChargerServer = ChargerServer
        self.charger_impl = ChargerServer.charger_impl
        self.sub_tcp = self.create_subscription(
            String, "/charger_server/cmd", self.listener_callback, 10
        )
        # Test:命令控制
        if Sim_Cmd_Control:
            self.control_cmd_count = 0
            self.control_client_id = 0
            self.sub_cmd = self.create_subscription(
                String, "/charger_cmd", self.control_cmd, 2
            )

    def listener_callback(self, msg):
        """
        msg.data 字符串格式:
        "cmd": "connect",
        "voltage": 145, 单位:0.1V
        "current": 100, 单位:0.1A
        "battery_type": 0x3,
        "station_id": 0x1,
        """
        args = json.loads(msg.data)
        chargingArgs = ChargingArgs()
        if args["cmd"] == "connect":
            chargingArgs.cmd_code = E_ChargingCmd.Connect
        elif args["cmd"] == "charging":
            chargingArgs.cmd_code = E_ChargingCmd.Charging
        elif args["cmd"] == "stop":
            chargingArgs.cmd_code = E_ChargingCmd.Stop
        elif args["cmd"] == "disconnect":
            chargingArgs.cmd_code = E_ChargingCmd.Disconnect
        chargingArgs.cmd = args["cmd"]
        chargingArgs.voltage = args["voltage"]
        chargingArgs.current = args["current"]
        chargingArgs.battery_type = args["battery_type"]
        chargingArgs.station_id = args["station_id"]
        if Sim_Cmd_Control:
            self.control_chargingArgs = chargingArgs
        logger.warning(f"收到充电指令: {chargingArgs}")
        self.charger_impl.hal_control_charging(chargingArgs)

    def choose_client_id(self, AGV_impl):
        clients = []
        idx = 0
        for key in AGV_impl.client_set:
            print(f"idx:{idx}, client_id: 0x{key:02X}")
            idx += 1
            clients.append(key)

        clnt_num = len(clients)
        if clnt_num == 0:
            print("没有连接的客户端")
            return 0
        elif clnt_num == 1:
            idx = 0
        else:
            print("请输入 idx")
            input_str = input().strip()
            idx = int(input_str)

        self.control_client_id = clients[idx]
        print(f"设置为client_id: {self.control_client_id:02X}")
        return self.control_client_id

    def control_cmd(self, msg):
        help_str = """
p 选择充电器
1 准备对接
2 开始充电
3 结束充电
4 取消充电
"""
        if self.control_cmd_count % 10 == 0:
            # 定时打印 使用帮助
            print(help_str)
        self.control_cmd_count += 1

        charger_impl = self.ChargerServer.charger_impl
        client_id = self.control_client_id
        key = msg.data[0]
        print("key:", key)
        if key == "p":
            client_id = self.choose_client_id(charger_impl)
        elif key == "2":
            chargingArgs = self.control_chargingArgs
            chargingArgs.cmd = "charging"
            chargingArgs.cmd_code = E_ChargingCmd.Charging
            charger_impl.hal_control_charging(chargingArgs)
        elif key == "3":
            chargingArgs = self.control_chargingArgs
            chargingArgs.cmd = "stop"
            chargingArgs.cmd_code = E_ChargingCmd.Stop
            charger_impl.hal_control_charging(chargingArgs)
        elif key == "4":
            chargingArgs = self.control_chargingArgs
            chargingArgs.cmd = "disconnect"
            chargingArgs.cmd_code = E_ChargingCmd.Disconnect
            charger_impl.hal_control_charging(chargingArgs)
        else:
            print("未知指令:", key)
